#include "HoughSpectrum.hh"
#include <cmath>
#include <cassert>

using namespace std;

HoughSpectrum::HoughSpectrum(double thetaStep,double rhoStep,double rhoMax)
 : thetaStep_(thetaStep), rhoStep_(rhoStep),
   thetaNum_(static_cast<unsigned int>(ceil(M_PI/thetaStep))), 
   rhoNum_(2 * static_cast<unsigned int>(ceil(rhoMax/rhoStep))),
   houghSpace_(thetaNum_ * rhoNum_,0),
   houghSpectrum_(thetaNum_,0),
   maxIdx_(0)
{
}
  
void HoughSpectrum::compute(const line::point_vector& pv)
{
  fill(houghSpace_.begin(),houghSpace_.end(),0);
  fill(houghSpectrum_.begin(),houghSpectrum_.end(),0);
  
  // Traces for each point pv[i] the associated curve in the Hough space,
  // i.e. the corresponding accumulator cells are incremented
 // cerr << __FILE__ << "," << __LINE__ << ": computing Hough Transform" << endl;
  for (unsigned int i = 0; i < pv.size(); ++i) {
    //cerr << endl << "point " << i << endl;
    for (unsigned int j = 0; j < thetaNum_; ++j) {
      double theta = -M_PI_2 + thetaStep_ * j;
      double rho = pv[i].x * cos(theta) + pv[i].y * sin(theta);
      unsigned int idx = thetaRhoToIdx(theta,rho);
//       cerr << "theta: " << theta << ", rho: " << rho << " index " << idx
//            << ", max index " << houghSpace_.size() << endl;
      if (idx < houghSpace_.size()) {
        houghSpace_[idx]++;
      }
    }
  }
  
  // Computes the Hough spectrum
 // cerr << __FILE__ << "," << __LINE__ << ": computing Hough Spectrum" << endl;
  maxIdx_ = 0;
  for (unsigned int itheta = 0; itheta < thetaNum_; ++itheta) {
    houghSpectrum_[itheta] = 0;
    for (unsigned int irho = 0; irho < rhoNum_; ++irho) {
      unsigned int idx = thetaRhoBinToIdx(itheta,irho);
      houghSpectrum_[itheta] += houghSpace_[idx] * houghSpace_[idx];
    }
    if (spectrumValue(maxIdx_) < spectrumValue(itheta)) {
      maxIdx_ = itheta;
    }
  }
}

unsigned int HoughSpectrum::houghValue(double theta,double rho) const
{
  unsigned int idx = thetaRhoToIdx(theta,rho);
  if (idx >= houghSpace_.size()) return 0;
  return houghSpace_[idx];
}
  
unsigned int HoughSpectrum::houghValue(unsigned int itheta,unsigned int irho) const
{
  unsigned int idx = thetaRhoBinToIdx(itheta,irho);
  if (idx >= houghSpace_.size()) return 0;
  return houghSpace_[idx];
}
  
unsigned int HoughSpectrum::spectrumValue(double theta) const
{
  unsigned int itheta = thetaToIdx(theta);
  if (itheta >= houghSpectrum_.size()) return 0;
  return houghSpectrum_[itheta];
}

unsigned int HoughSpectrum::spectrumValue(unsigned int itheta) const
{
  if (itheta >= houghSpectrum_.size()) return 0;
  return houghSpectrum_[itheta];
}

double HoughSpectrum::spectrumMax() const 
{
  return idxToTheta(maxIdx_);
}

unsigned int HoughSpectrum::spectrumMaxIdx() const 
{
  return maxIdx_;
}

unsigned int HoughSpectrum::thetaBinNum() const
{
  return houghSpectrum_.size();
}

unsigned int HoughSpectrum::rhoBinNum() const
{
  return rhoNum_;
}

unsigned int HoughSpectrum::thetaRhoBinToIdx(unsigned int itheta,unsigned int irho) const
{
  assert(itheta <= thetaNum_);
  assert(irho <= rhoNum_);
  return (irho * thetaNum_ + itheta);
}

unsigned int HoughSpectrum::thetaRhoToIdx(double theta,double rho) const
{
  unsigned int itheta;
  unsigned int irho;   

//   double thetaNorm;  
//   if (cos(theta) > 0.0) 
//     thetaNorm = atan2(sin(theta),cos(theta));
//   else
//     thetaNorm = atan2(-sin(theta),-cos(theta));
//   itheta = static_cast<unsigned int>(round((thetaNorm + M_PI_2)/thetaStep_));
//   irho = static_cast<unsigned int>(round(rho / rhoStep_)) + rhoNum_/2;
  
  itheta = thetaToIdx(theta);
  irho = rhoToIdx(rho);
//   cerr << "theta " << theta << ", rho " << rho 
//        << ", itheta " << itheta << ", irho " << irho 
//       << " thetaNum " << thetaNum_ << ", rhoNum " << rhoNum_ << endl;

  if (itheta > thetaNum_ || irho > rhoNum_) 
    return houghSpace_.size();
  return (irho * thetaNum_ + itheta);
}

unsigned int HoughSpectrum::thetaToIdx(double theta) const
{
  double thetaNorm;
  unsigned int itheta;
  if (cos(theta) > 0.0) 
    thetaNorm = atan2(sin(theta),cos(theta));
  else
    thetaNorm = atan2(-sin(theta),-cos(theta));
  itheta = static_cast<unsigned int>(round((thetaNorm + M_PI_2)/thetaStep_));
  return itheta;
}

double HoughSpectrum::idxToTheta(unsigned int itheta) const
{
  return (thetaStep_ * itheta - M_PI_2);
}

unsigned int HoughSpectrum::rhoToIdx(double rho) const
{
  unsigned int irho = static_cast<unsigned int>(round(rho / rhoStep_)) + rhoNum_/2;
  return irho;
}
  
double HoughSpectrum::idxToRho(unsigned int irho) const
{
  double rho = rhoStep_ * irho - rhoStep_ * (rhoNum_ / 2);
  return rho;
}


